//------------------------------------------------------------------------------
//  <copyright file="KukaLBR4Operations.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once

#include "KukaLBR4PlusControllerExports.h"

namespace KukaLBR4OperationsNS
{
    /// <remarks>
    /// These functions define the operations available to the controller state machine
    /// as either "state operations" which are performed on every tick that the controller
    /// is in a given state, "transition tests" which are used to determine when a state
    /// transition should occur, or "transition operations" which occur on state transitions.
    /// The parameters to these operations are arrays of pointers that point to state machine
    /// registers.
    /// </remarks>

	/// <summary>
	/// Register as KukaLbr4OperationsProvider
	/// </summary>
	void RegisterKukaLbr4OperationsProvider();

	/// <summary>
	/// Unregister as KukaLbr4OperationsProvider
	/// </summary>
	void UnregisterKukaLbr4OperationsProvider();

    /// <summary>
    /// Adds two scalar values.
    /// ppParameters[0] = ppParameters[1] + ppParameters[2]
    /// <summary>
    /// <param name="ppParameters">
    /// Array of pointers to parameters for operation.
    /// ppParameters[0]:  Length 1, destination for operation result;
    /// ppParameters[1]:  Length 1.
    /// ppParameters[2]:  Length 1.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAdd(double **ppParameters);

    /// <summary>
    /// Adds two vectors of length 6
    /// ppParameters[0] = ppParameters[1] + ppParameters[2]
    /// <summary>
    /// <param name="ppParameters">
    /// Array of pointers to parameters for operation.
    /// ppParameters[0]:  Length 6, destination for operation result;
    /// ppParameters[1]:  Length 6.
    /// ppParameters[2]:  Length 6.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAddVec6(double **ppParameters);

    /// <summary>
    /// Multiplies two scalar values.
    /// ppParameters[0] = ppParameters[1] * ppParameters[2]
    /// <summary>
    /// <param name="ppParameters">
    /// Array of pointers to parameters for operation.
    /// ppParameters[0]:  Length 1, destination for operation result;
    /// ppParameters[1]:  Length 1.
    /// ppParameters[2]:  Length 1.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpMultiply(double **ppParameters);

    /// <summary>
    /// Sets paramters[0] to 1 if the vector magnitude of parameter[1] is greater than parameter[2], otherwise
    /// sets parameter[0] to 0.
    /// <summary>
    /// <param name="ppParameters">
    /// Array of pointers to parameters for operation.
    /// ppParameters[0]:  Length 1, destination for operation result;
    /// ppParameters[1]:  Length 6.
    /// ppParameters[2]:  Length 1.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsMagnitudeGreaterThanVec6(double **ppParameters);

    /// <summary>
    /// Sets paramters[0] to 1 if any element of parameter[1] is greater than the corresponding
    /// element of paramter[2], otherwise sets parameter[0] to 0.
    /// <summary>
    /// <param name="ppParameters">
    /// Array of pointers to parameters for operation.
    /// ppParameters[0]:  Length 1, destination for operation result;
    /// ppParameters[1]:  Length 6.
    /// ppParameters[2]:  Length 6.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsAnyElementGreaterThanVec6(double **ppParameters);

    /// <summary>
    /// Copes a vector of length 7 to another vector of length 7
    /// parameters[0] = parameters[1]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 7, destination for operation result;
    /// parameters[1]:  Length 7.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyVec7(double **parameters);

	/// <summary>
    /// Sets parameter[0] to the next goal given current value, max speed, and cycle rate (in ms)
    /// </summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 12, next value.
    /// parameters[1]:  Length 12, current value.
    /// parameters[2]:  Length 12, desired valre.
    /// parameters[3]:  Length 1, max joint velocity in radians/s
    /// parameters[4]:  Length 1, cycle time in ms
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSimpleCartesianInterpolation(double **parameters);

	 /// <summary>
    /// Sets parameter[0] to the next goal given current value, max speed, and cycle rate (in ms)
    /// </summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 7, next value.
    /// parameters[1]:  Length 7, current value.
    /// parameters[2]:  Length 7, desired valre.
    /// parameters[3]:  Length 1, max joint velocity in radians/s
    /// parameters[4]:  Length 1, cycle time in ms
	/// parameters[5]:  Length 1, duration of motion in cycles
	/// parameters[6]:  Length 1, current cycle number
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSimpleJointInterpolation(double **parameters);

    /// <summary>
    /// Sets parameter[0] to 1 if parameter[1] == parameter[2].  Otherwise sets parameter[0] to 0.
    /// </summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 1, destination for operation result;
    /// parameters[1]:  Length 1.
    /// parameters[2]:  Length 1.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAreScalarsEqual(double **parameters);

    /// <summary>
    /// Adds two vectors of length 7
    /// parameters[0] = parameters[1] + parameters[2]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 7, destination for operation result;
    /// parameters[1]:  Length 7.
    /// parameters[2]:  Length 7.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAddVec7(double **parameters);

    /// <summary>
    /// Subtracts two vectors of length 7
    /// parameters[0] = parameters[1] - parameters[2]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 7, destination for operation result;
    /// parameters[1]:  Length 7.
    /// parameters[2]:  Length 7.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSubtractVec7(double **parameters);

    /// Copies a vector another vector of identical length. Length is passed as parameter
    /// parameters[0] = parameters[1]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 7, destination for operation result;
    /// parameters[1]:  Length 7.
    /// parameters[2]:  Vector length
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyVec(double **parameters);

	/// <summary>
    /// Copies a vector of length 6 to another vector of length 6
    /// parameters[0] = parameters[1]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 6, destination for operation result;
    /// parameters[1]:  Length 6.
    /// </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyVec6(double **parameters);

	/// <summary>
    /// Copies a vector of length 12 to another vector of length 12 (length of a frame vecotr) 
    /// parameters[0] = parameters[1]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 12, destination for operation result;
    /// parameters[1]:  Length 12.
    /// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyFrame(double **parameters);

	/// <summary>
    /// Multiplies two frames represented as row-major arrays.
    /// parameters[0] = parameters[1] * parameters[2]
    /// <summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 12, destination for operation result;
    /// parameters[1]:  Length 12, frame for LHS of mulitplication
    /// parameters[2]:  Length 12, frame for RHS of multiplication
    /// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpMultiplyFrames(double **parameters);

	/// <summary>
	/// Takes a force-torque (6-dim) vector as supplied by LBR and returns 1 if the magnitude 
	/// of the 3-vector given by the first 3 elements (the X-Y-Z forces at the end point) is
	/// greater than the provided scalar.  
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
    /// parameters[0]:  Length 1. Will be 1 if force magnitude is greater than supplied threshold, zero otherwise.
    /// parameters[1]:  Length 6. [X, Y, Z, A, B, C] endpoint force-torques as given by Kuka LBR.
    /// parameters[2]:  Length 1. Scalar threshold for comparison to force magnitude.
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsForceMagnitudeGreaterThanScalar(double **parameters);

	

	/// <summary> Get the projection of a 3-vector onto a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="parameters">
	/// parameters[0]:  Length 1.  Result.  Will be one if the force in parameters[1] projected into the plane orthogonal to 
	///								parameters[2] is greater than the threshold given in parameter[3].
	/// parameters[1]:  Length 3.  The Cartesian force vector (X, Y, Z).
	/// parameters[2]:  Length 3.  The unit vector normal to the plane to project the force in prameters[1] onto.
	/// parameters[3]:  Length 1.  Force magnitude threshold.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeIsForceInPlaneGreaterThanScalar(double **parameters);

	/// <summary> Get the rejection of a 3-vector from a a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="parameters">
	/// parameters[0]:  Length 1.  Result.  Will be one if the force in parameters[1] rejected from the plane orthogonal to 
	///								parameters[2] is greater than the threshold given in parameter[3].
	/// parameters[1]:  Length 3.  The Cartesian force vector (X, Y, Z).
	/// parameters[2]:  Length 3.  The unit vector normal to the plane to reject the force in prameters[1] from.
	/// parameters[3]:  Length 1.  Force magnitude threshold.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeIsForceOrthogonalToPlaneGreaterThanScalar(double **parameters);

    /// <summary>
    /// Move the arm to the "candle" position
    /// </summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length numDim, next step command.
    /// parameters[1]:  Length numDim, current value.
    /// parameters[2]:  Length 1, desired joint velocity in units/s
    /// parameters[3]:  Length 1, cycle time in ms
    /// </param>
	/// <param name="numDim"> Number of dimensions to interpolate. </param>
    KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeGetJointAnglesForMotionToCandleAtConstantRate(double **parameters);

	/// <summary>
    /// Sets parameter[0] to the next goal given current value, max speed, and cycle rate (in ms)
    /// </summary>
    /// <param name="parameters">
    /// Array of pointers to parameters for operation.
    /// parameters[0]:  Length numDim, next step command.
    /// parameters[1]:  Length numDim, current value.
    /// parameters[2]:  Length numDim, desired value.
    /// parameters[3]:  Length numDim, max velocity in units/s
    /// parameters[4]:  Length numDim, cycle time in ms
	/// parameters[5]:  Length 1, duration of motion in cycles
	/// parameters[6]:  Length 1, current cycle number
    /// </param>
	/// <param name="numDim"> Number of dimensions to interpolate. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void SimpleInterpolation(double **parameters, int numDim);

	/// <summary> Convert the rotational portion of a frame descriptor to a quaternion </summary>
	/// <param name="pFrame"> A 3x4 frame descriptor in row-major order </param>
	/// <param name="pQuat"> 
	/// Pointer to location to store the resulting quaternion (4 dimensional).
	/// Quaternion is stored as [W, X, Y, Z], where W is the scalar component.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void FrameToQuaternion(double *pFrame, double *pQuat);

	/// <summary> Returns -1 if sign of parameter is negative, 0 otherwise </summary>
	/// <param name="value"> Value to get the sign of. </param>
	/// <returns> -1 if value < 0, +1 otherwise. </returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT double Sign(double value);

	/// <summary>Normalize a quaternion to the unit quaternion</summary>
	/// <param name="pQuatIn">Quaternion of form [W, X, Y, Z] to normalize</param>
	/// <param name="pQuatNorm>Normalized output quaternion in form [W, X, Y, Z]</param>
	/// <returns>false if quaternion cannot be normalized (all zeroes)</returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT bool QuaternionNormalize(double *pQuatIn, double *pQuatNorm);

	/// <summary> Convert a quaternion to the rotational component of a frame description.  </summary>
	/// <param name="pQuat"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pFrame>
	/// Pointer to location to store the resulting rotational component of the frame in row-major order.
	/// Entries correpsonding to position will not be modified.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionToFrame(double *pQuat, double *pFrame);

	/// <summary> Returns the dot product of two quaternions. </summary>
	/// <param name="pQuat1"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuat2"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <returns> Dot product of two quaternions. </returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT double QuaternionDotProduct(double *pQuat1, double *pQuat2);

	/// <summary> Gets the sum  of two quaternions. </summary>
	/// <param name="pQuat1"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuat2"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuatResult"> Location where sum will be placed in the form of [W, X, Y, Z] (W is scalar component). </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionSum(double *pQuat1, double *pQuat2, double *pQuatResult);

	/// <summary> Gets the product of a scalar and a quaternion. </summary>
	/// <param name="scalar"> A scalar value. </param>
	/// <param name="pQuat"> An quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuatResult"> Location where product will be placed in the form of [W, X, Y, Z] (W is scalar component). </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionScalarProduct(double scalar, double *pQuat, double *pQuatResult);

	/// <summary> Computes the vector dot product of A and B. </summary>
	/// <param name="pA"> Vector A. </param>
	/// <param name="pB"> Vector B. </param>
	/// <param name="length"> Length of vectors A and B. </param>
	/// <param name="pResult"> Location to place the result of A dot B. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void DotProduct(double *pA, double *pB, int length, double *pResult);

	/// <summary> Gets the cross product of pVec1 and pVec2, placing the result in pResult. </summary>
	/// <param name="pVec1"> A 3-vector. </param>
	/// <param name="pVec2"> A 3-vector. </param>
	/// <param name="pResult"> Location to place the result of the cross product of pVec1 and pVec2. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vec3CrossProduct(double *pVec1, double *pVec2, double *pResult);

	/// <summary> Gets the magnitude of a vector. </summary>
	/// <param name="pVec"> Vector to get the magnitude of. </param>
	/// <param name="length"> Number of elements in the vector. </param>
	/// <param name="pResult"> Location to place the mangnitude result. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void VectorMagnitude(double *pVec, int length, double *pResult);

	/// <summary> Get the projection of one 3-vector onto another. </summary>
	/// <param name="pVec1">  The vector to be projected. </param>
	/// <param name="pVec2">  The vector to project pVec1 onto.  Must be a unit vector. </param>
	/// <param name="pResult">  The result of projecting pVec1 onto pVec2. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vector3Vector3Project(double *pVec1, double *pVec2, double *pResult);

	/// <summary> Get the projection of a 3-vector onto a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="pVec">  The vector to be projected onto the plane normal to pPlaneVec. </param>
	/// <param name="pPlaneVec">  The unit vector normal to the plane to project pVec onto. </param>
	/// <param name="pResult">  The result of projecting pVec onto the plane normal to pPlaneVec. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vector3PlaneProject(double *pVec, double *pPlaneVec, double *pResult);
}
